import psutil
import subprocess
import signal
import time

def kill_process_tree(pid, timeout=15):
    """杀死进程树"""
    try:
        parent = psutil.Process(pid)
        procs = parent.children(recursive=True) + [parent]
        
        # 1. 温柔终止
        for p in procs:
            try:
                p.terminate()
            except psutil.NoSuchProcess:
                continue
        
        gone, alive = psutil.wait_procs(procs, timeout=timeout//2)
        
        # 2. 强制杀死
        for p in alive:
            try:
                p.kill()
            except psutil.NoSuchProcess:
                continue
        
        gone, alive = psutil.wait_procs(alive, timeout=timeout//2)
        
    except psutil.NoSuchProcess:
        pass
    except Exception as e:
        print(f"终止进程时出错: {e}")

def kill_all_ros_processes():
    """杀死所有ROS相关进程"""
    try:
        # 杀死核心ROS进程
        subprocess.run(['pkill', '-f', 'roscore'], stderr=subprocess.DEVNULL)
        subprocess.run(['pkill', '-f', 'rosmaster'], stderr=subprocess.DEVNULL)
        subprocess.run(['pkill', '-f', 'rosout'], stderr=subprocess.DEVNULL)
        time.sleep(1)
        
    except Exception as e:
        print(f"清理ROS进程时出错: {e}")